package commandsimtest01;

import java.util.Scanner;

public class CommandSimTest01 {

    static double launchLatitude=-52.80261, launchLongitude=-58.358615, targetLatitude=-52.97188, targetLongitude=-57.671332;
    static double groundRange, forwardAzimuth, distanceFromLaunch;
    static double latitudeN, longitudeN;//temp gps points
    static double gpsResolution = 1.0;
    static String flightPath=null;
    static StringBuffer sb = new StringBuffer();
    

    public static void main(String[] args) {
        Scanner console = new Scanner(System.in);
        giveIntro();//part 1
        System.out.print("Enter Launch Latitude:");
     //   launchLatitude = console.nextDouble();
        System.out.print("Enter Launch Longitude:");
     //   launchLongitude = console.nextDouble();
        System.out.print("Enter Target Latitude:");
     //   targetLatitude = console.nextDouble();
        System.out.print("Enter Target Longitude:");
    //    targetLongitude = console.nextDouble();
        System.out.println("----------------------------------------------------------");

        onBoardComputer();
        results();



    }

    public static void onBoardComputer() {
        groundRange = getGroundRange(launchLatitude, launchLongitude, targetLatitude, targetLongitude);
        forwardAzimuth = getForwardAzimuth(launchLatitude, launchLongitude, targetLatitude, targetLongitude);
        
        do{
        latitudeN = getNextLatitude(launchLatitude, launchLongitude, forwardAzimuth, distanceFromLaunch);
        longitudeN = getNextLongitude(launchLatitude, launchLongitude, forwardAzimuth, distanceFromLaunch);
            sb.append("(").append(latitudeN).append("°\t").append(longitudeN).append("°)\n");
        distanceFromLaunch+=1;
        }
        while(distanceFromLaunch < groundRange);   
        
            
        

    }

    public static void results() {
        System.out.printf("Ground range to target = %3.5f Km", groundRange / 1000);
        System.out.printf("\nForward azimuth to target = %3.5f°\n", forwardAzimuth);
        System.out.printf("\nNext point Latitude onroute to target = %3.5f°\n", latitudeN);
        System.out.printf("\nNext point Longitude onroute to target = %3.5f°\n", longitudeN);
        System.out.println("\nFlight Path is: \n"+ sb.substring(0, sb.length()));
      //  System.out.println("\nFlight Path is: \n"+ sb.substring(sb.length()/2,sb.length()));

        System.out.printf("Original target point:(%3.7f°, %3.7f°)\n", targetLatitude,targetLongitude);
        
    }

    public static double getNextLatitude(double latitudeA, double longitudeA, double forwardAzimuth, double range) {
        //double[] latANDlon = new double[2];
        double a = 6378160;//South American (1969) ellipsoid model
        double b = 6356774.719;//South American (1969) ellipsoid model
        double f = (a - b) / a;//South American (1969) ellipsoid model
        double s = range;
        double alpha1 = Math.toRadians(forwardAzimuth);
        double sinAlpha1 = Math.sin(alpha1);
        double cosAlpha1 = Math.cos(alpha1);
        double tanU1 = (1 - f) * Math.tan(Math.toRadians(latitudeA));
        double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1)), sinU1 = tanU1 * cosU1;
        double sigma1 = Math.atan2(tanU1, cosAlpha1);
        double sinAlpha = cosU1 * sinAlpha1;
        double cosSqAlpha = 1 - sinAlpha * sinAlpha;
        double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        double sigma = s / (b * A), sigmaP = 2 * Math.PI;
        double sinSigma = Math.sin(sigma);
        double cosSigma = Math.cos(sigma);
        double cos2SigmaM = Math.cos(2 * sigma1 + sigma);
        while (Math.abs(sigma - sigmaP) > 1e-12) {
            cos2SigmaM = Math.cos(2 * sigma1 + sigma);
            sinSigma = Math.sin(sigma);
            cosSigma = Math.cos(sigma);
            double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
                    - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
            sigmaP = sigma;
            sigma = s / (b * A) + deltaSigma;
        }

        double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
        double nextLatitude = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
                (1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp));
        double lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
        double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
        double L = lambda - (1 - C) * f * sinAlpha
                * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
        double nextLongitude = (Math.toRadians(longitudeA) + L + 3 * Math.PI) % (2 * Math.PI) - Math.PI;  // normalise to -180...+180

        //latANDlon[0] = Math.toDegrees(nextLatitude);
        //latANDlon[1] = Math.toDegrees(nextLongitude);
        // latANDlonANDRevAz[2] = Math.toDegrees(Math.atan2(sinAlpha, -tmp));  // final bearing, if required

        return Math.toDegrees(nextLatitude);
    }

    public static double getNextLongitude(double latitudeA, double longitudeA, double forwardAzimuth, double range) {
        //double[] latANDlon = new double[2];
        double a = 6378160;//South American (1969) ellipsoid model
        double b = 6356774.719;//South American (1969) ellipsoid model
        double f = (a - b) / a;//South American (1969) ellipsoid model
        double s = range;
        double alpha1 = Math.toRadians(forwardAzimuth);
        double sinAlpha1 = Math.sin(alpha1);
        double cosAlpha1 = Math.cos(alpha1);
        double tanU1 = (1 - f) * Math.tan(Math.toRadians(latitudeA));
        double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1)), sinU1 = tanU1 * cosU1;
        double sigma1 = Math.atan2(tanU1, cosAlpha1);
        double sinAlpha = cosU1 * sinAlpha1;
        double cosSqAlpha = 1 - sinAlpha * sinAlpha;
        double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        double sigma = s / (b * A), sigmaP = 2 * Math.PI;
        double sinSigma = Math.sin(sigma);
        double cosSigma = Math.cos(sigma);
        double cos2SigmaM = Math.cos(2 * sigma1 + sigma);
        while (Math.abs(sigma - sigmaP) > 1e-12) {
            cos2SigmaM = Math.cos(2 * sigma1 + sigma);
            sinSigma = Math.sin(sigma);
            cosSigma = Math.cos(sigma);
            double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
                    - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
            sigmaP = sigma;
            sigma = s / (b * A) + deltaSigma;
        }

        double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
        double nextLatitude = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
                (1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp));
        double lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
        double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
        double L = lambda - (1 - C) * f * sinAlpha
                * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
        double nextLongitude = (Math.toRadians(longitudeA) + L + 3 * Math.PI) % (2 * Math.PI) - Math.PI;  // normalise to -180...+180

        // latANDlon[0] = Math.toDegrees(nextLatitude);
        //    latANDlon[1] = Math.toDegrees(nextLongitude);
        // latANDlonANDRevAz[2] = Math.toDegrees(Math.atan2(sinAlpha, -tmp));  // final bearing, if required

        return Math.toDegrees(nextLongitude);
    }

    public static double getForwardAzimuth(double latitudeA, double longitudeA, double latitudeB, double longitudeB) {
        //   double[] rangeANDFwAzANDRevAz = new double[3];
        double a = 6378160;//South American (1969) ellipsoid model
        double b = 6356774.719;//South American (1969) ellipsoid model
        double f = (a - b) / a;//South American (1969) ellipsoid model
        double L = Math.toRadians(longitudeB - longitudeA);
        double U1 = Math.atan((1 - f) * Math.tan(Math.toRadians(latitudeA)));
        double U2 = Math.atan((1 - f) * Math.tan(Math.toRadians(latitudeB)));
        double sinU1 = Math.sin(U1), cosU1 = Math.cos(U1);
        double sinU2 = Math.sin(U2), cosU2 = Math.cos(U2);
        double lambda = L, lambdaP, iterLimit = 100;
        double sinLambda = Math.sin(lambda);
        double cosLambda = Math.cos(lambda);
        double sinSigma = Math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda)
                + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
        double cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        double sigma = Math.atan2(sinSigma, cosSigma);
        double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
        double cosSqAlpha = 1 - sinAlpha * sinAlpha;
        double cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;

        do {
            sinLambda = Math.sin(lambda);
            cosLambda = Math.cos(lambda);
            sinSigma = Math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda)
                    + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
            if (sinSigma == 0) {
                System.out.println("co-incident points");
            }
            cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
            sigma = Math.atan2(sinSigma, cosSigma);
            sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
            cosSqAlpha = 1 - sinAlpha * sinAlpha;
            cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
            if (Double.isNaN(cos2SigmaM)) {
                cos2SigmaM = 0;  // equatorial line: cosSqAlpha=0 (§6)
            }
            double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
            lambdaP = lambda;
            lambda = L + (1 - C) * f * sinAlpha
                    * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
        } while (Math.abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

        if (iterLimit == 0) {
            System.out.println("NaN iterLimit==0 \n formula failed to converge");
        }
        double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
                - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
        //  double s = b * A * (sigma - deltaSigma);

        //   rangeANDFwAzANDRevAz[0] = s; // range 


        // note: to return initial/final bearings in addition to distance, use something like:
        double fwdAz = Math.atan2(cosU2 * sinLambda, cosU1 * sinU2 - sinU1 * cosU2 * cosLambda);
        //double revAz = Math.atan2(cosU1 * sinLambda, -sinU1 * cosU2 + cosU1 * sinU2 * cosLambda);

        //rangeANDFwAzANDRevAz[1] = Math.toDegrees(fwdAz);
        //rangeANDFwAzANDRevAz[2] = Math.toDegrees(revAz);
        //return rangeANDFwAzANDRevAz;

        return Math.toDegrees(fwdAz);
    }

    public static double getGroundRange(double latitudeA, double longitudeA, double latitudeB, double longitudeB) {
        //   double[] rangeANDFwAzANDRevAz = new double[3];
        double a = 6378160;//South American (1969) ellipsoid model
        double b = 6356774.719;//South American (1969) ellipsoid model
        double f = (a - b) / a;//South American (1969) ellipsoid model
        double L = Math.toRadians(longitudeB - longitudeA);
        double U1 = Math.atan((1 - f) * Math.tan(Math.toRadians(latitudeA)));
        double U2 = Math.atan((1 - f) * Math.tan(Math.toRadians(latitudeB)));
        double sinU1 = Math.sin(U1), cosU1 = Math.cos(U1);
        double sinU2 = Math.sin(U2), cosU2 = Math.cos(U2);
        double lambda = L, lambdaP, iterLimit = 100;
        double sinLambda = Math.sin(lambda);
        double cosLambda = Math.cos(lambda);
        double sinSigma = Math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda)
                + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
        double cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        double sigma = Math.atan2(sinSigma, cosSigma);
        double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
        double cosSqAlpha = 1 - sinAlpha * sinAlpha;
        double cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;

        do {
            sinLambda = Math.sin(lambda);
            cosLambda = Math.cos(lambda);
            sinSigma = Math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda)
                    + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
            if (sinSigma == 0) {
                System.out.println("co-incident points");
            }
            cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
            sigma = Math.atan2(sinSigma, cosSigma);
            sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
            cosSqAlpha = 1 - sinAlpha * sinAlpha;
            cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
            if (Double.isNaN(cos2SigmaM)) {
                cos2SigmaM = 0;  // equatorial line: cosSqAlpha=0 (§6)
            }
            double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
            lambdaP = lambda;
            lambda = L + (1 - C) * f * sinAlpha
                    * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
        } while (Math.abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

        if (iterLimit == 0) {
            System.out.println("NaN iterLimit==0 \n formula failed to converge");
        }
        double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
                - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
        double s = b * A * (sigma - deltaSigma);

        //   rangeANDFwAzANDRevAz[0] = s; // range 


        // note: to return initial/final bearings in addition to distance, use something like:
        // double fwdAz = Math.atan2(cosU2 * sinLambda, cosU1 * sinU2 - sinU1 * cosU2 * cosLambda);
        //double revAz = Math.atan2(cosU1 * sinLambda, -sinU1 * cosU2 + cosU1 * sinU2 * cosLambda);

        //rangeANDFwAzANDRevAz[1] = Math.toDegrees(fwdAz);
        //rangeANDFwAzANDRevAz[2] = Math.toDegrees(revAz);
        //return rangeANDFwAzANDRevAz;

        return s;
    }

    public static void giveIntro() {
        System.out.println("This program computes the following:");
        System.out.println("(1) The ground range distance between  two GPS points.");
        System.out.println("(2) The Azimuth between the two GPS points");
        System.out.println("(3) A Track between the two points at 1 meter resolution.");
        System.out.println("----------------------------------------------------------");

    }
}
